#include "PosConvert.h"



int PosConvert::Save_Ori_Pts()
{
    // 写入数据到文件
    ofstream outFile(ori_pt_file,std::ios::trunc);

    if(outFile.is_open()) 
    {
        logger_->info("Saving original points , original points file open succ ~~");
        cout << "原点坐标文件打开成功！" << endl;
    } 
    else 
    {
        logger_->error("Saving original points , original points file open failed !");
        cerr << "原点坐标文件无法创建！" << endl;
        return -1;
    }

    // for (int i = 0; i < NUM_MOTOR; ++i) 
    // {
    //     outFile << pts_ori[i];

    //     if (i != NUM_MOTOR - 1) 
    //     {  // 最后一个元素不加分号
    //         outFile << ";";
    //     }
    // }
    outFile << OriPts.ex;
    outFile << ";";
    outFile << OriPts.ey;
    outFile << ";";
    outFile << OriPts.ez_1;
    outFile << ";";
    outFile << OriPts.ez_2;
    outFile << ";";
    outFile << OriPts.erx;
    outFile << ";";
    outFile << OriPts.erz;

    cout << "原点坐标保存成功" << endl;

    outFile.close();

    return TP_DN_STATUS_SUCCES;

}

int PosConvert::Load_Ori_Pts()
{
    // 从文件读取数据
    ifstream inFile(ori_pt_file);
    string line;

    if (inFile.is_open()) 
    {
        logger_->debug("Load original potins file SUCCCESS !! ");
        getline(inFile, line);
        inFile.close();
    } 
    else 
    {
        logger_->error("Load original potins, cannot open file !");
        // cerr << "无法打开文件！" << endl;
        return -1;
    }

    // 分割字符串
    vector<string> data = split(line, ';');

    if (data.size() != NUM_MOTOR) 
    {
        logger_->error("Data format error! Six semicolon-separated values are required !");
        // cerr << "数据格式错误!需要6个分号分隔的值" << endl;
        return -2;
    }

    // 将字符串转换为变量（示例使用不同类型）
    try {
        // int var1 = stoi(data[0]);
        // double var2 = stod(data[1]);

        // pts_ori_r[0] = stoi(data[0]);
        // pts_ori_r[1] = stoi(data[1]);
        // pts_ori_r[2] = stoi(data[2]);
        // pts_ori_r[3] = stoi(data[3]);
        // pts_ori_r[4] = stoi(data[4]);
        // pts_ori_r[5] = stoi(data[5]);

        OriPts.ex = stoi(data[0]);
        OriPts.ey = stoi(data[1]);
        OriPts.ez_1 = stoi(data[2]);
        OriPts.ez_2 = stoi(data[3]);
        OriPts.erx = stoi(data[4]);
        OriPts.erz = stoi(data[5]);
 
        // 输出验证
        cout << "读取到的数据：" << endl;
        cout << "Ori_X: " << OriPts.ex << endl;
        cout << "Ori_Y: " << OriPts.ey << endl;
        cout << "Ori_Z_1: " << OriPts.ez_1 << endl;
        cout << "Ori_Z_2: " << OriPts.ez_2 << endl;
        cout << "Ori_RX: " << OriPts.erx << endl;
        cout << "Ori_RZ: " <<  OriPts.erz  << endl;

        logger_->info(" Read Originale Potins ------- ");
        logger_->info(" Ori_X: {}",OriPts.ex);
        logger_->info(" Ori_Y: {}",OriPts.ey);
        logger_->info(" Ori_Z_1: {}",OriPts.ez_1);
        logger_->info(" Ori_Z_2: {}",OriPts.ez_2);
        logger_->info(" Ori_RX: {}",OriPts.erx);
        logger_->info(" Ori_RZ: {}",OriPts.erz);
 
    } 
    catch (const exception& e) 
    {
        cerr << "数据类型转换错误: " << e.what() << endl;
        return -3;
    }
 
    return TP_DN_STATUS_SUCCES;

}


int PosConvert::Set_OriPts(tpDNOriginEncoderStruct & ori)   //保存坐标原点
{
    tpDNOriginEncoderStruct OriPts_Bak;

    OriPts_Bak = OriPts;

    OriPts = ori;

    // 输出验证
    cout << "待写入文件的原点坐标：" << endl;
    cout << "Ori_X: " << OriPts.ex << endl;
    cout << "Ori_Y: " << OriPts.ey << endl;
    cout << "Ori_Z1: " << OriPts.ez_1 << endl;
    cout << "Ori_Z2: " << OriPts.ez_2 << endl;
    cout << "Ori_RX: " << OriPts.erx << endl;
    cout << "Ori_RZ: " <<  OriPts.erz  << endl;

    if(Save_Ori_Pts() == TP_DN_STATUS_SUCCES)
    {
        return TP_DN_STATUS_SUCCES;
    }
    else
    {
        OriPts = OriPts_Bak;
        return -1;
    }
}


const tpDNPositionStruct& PosConvert::Refresh_Init_Pos(int32_t* inc_enc,int32_t* abs_enc)  //获取初始位置 (mm,deg单位)
{
    int i;

    int32_t del_enc_erx;
    int32_t del_enc_erz;
    float z_1,z_2;


    //静止时，全部采用绝对值编码器换算
    init_pos.x = ENC_2_MM_HC_ABS * (abs_enc[0] - OriPts.ex);
    init_pos.y = ENC_2_MM_HC_ABS * (abs_enc[1] - OriPts.ey);

    z_1 = ENC_2_MM_HC_ABS * (abs_enc[2] - OriPts.ez_1);
    z_2 = ENC_2_MM_HC_ABS * (abs_enc[3] - OriPts.ez_2);
    init_pos.z = (z_1 + z_2) / 2.0f;

    // del_enc_ery = -10000;
    del_enc_erx = static_cast<int32_t>(abs_enc[4] - OriPts.erx);
    if(del_enc_erx >= 0)
    {
        init_pos.rx = 360.0f - ((del_enc_erx * 1.0f) / ENC_ABS_PER_ROUND_YY) * 360.0f;
    }
    else
    {
        init_pos.rx = 360.0f - (((ENC_ABS_PER_ROUND_YY + del_enc_erx )* 1.0f) / ENC_ABS_PER_ROUND_YY) * 360.0f;
    }

    if(init_pos.rx >= 360)
    {
        init_pos.rx -= 360;
    }

    del_enc_erz = static_cast<int32_t>(abs_enc[5] - OriPts.erz);
    if(del_enc_erz >= 0)
    {
        init_pos.rz = 360.0f - ((del_enc_erz * 1.0f) / ENC_ABS_PER_ROUND_YY) * 360.0f;
    }
    else
    {
        init_pos.rz = 360.0f - (((ENC_ABS_PER_ROUND_YY + del_enc_erz )* 1.0f) / ENC_ABS_PER_ROUND_YY) * 360.0f;
    }

    if(init_pos.rz >= 360)
    {
        init_pos.rz -= 360;
    }
    // init_pos.ry = ENC_2_DEG_YY_ABS * (abs_enc[4] - OriPts.ery);
    // init_pos.rz = ENC_2_DEG_YY_ABS * (abs_enc[5] - OriPts.erz);

    for(i = 0; i < NUM_MOTOR; i++)
    {
        increment_enc_before_move[i] = inc_enc[i];
    }

    Is_First_Get_Pos = true;

    rt_pos = init_pos;
    
    return init_pos;
}


const tpDNPositionStruct& PosConvert::Refresh_Rt_Pos(int32_t* inc_enc, int32_t* abs_enc) //获取实时位置 (mm,deg单位)
{
    float z_1,z_2;

    rt_pos.x = ENC_2_MM_HC_ABS * (abs_enc[0] - OriPts.ex);   //禾川的绝对值编码器刷新频率快，直接用绝对值换算
    rt_pos.y = ENC_2_MM_HC_ABS * (abs_enc[1] - OriPts.ey);

    z_1 = ENC_2_MM_HC_ABS * (abs_enc[2] - OriPts.ez_1);
    z_2 = ENC_2_MM_HC_ABS * (abs_enc[3] - OriPts.ez_2);

    rt_pos.z = (z_1 + z_2) / 2.0f;

    rt_pos.rx = init_pos.rx + ENC_2_DEG_YY * (inc_enc[4] - increment_enc_before_move[4]);  //意优的绝对值编码器刷新频率慢，用增编换算
    rt_pos.rz = init_pos.rz + ENC_2_DEG_YY * (inc_enc[5] - increment_enc_before_move[5]);

    // if(rt_pos.rx >= 360)
    // {
    //     rt_pos.rx = 0;
    // }
    // if(rt_pos.rz >= 360)
    // {
    //     rt_pos.rz = 0;
    // }
    
    return rt_pos;
}

void PosConvert::Update_increment_enc_before_move(int32_t* inc_enc)
{
    int i;
    
    for(i = 0; i < NUM_MOTOR; i++)
    {
        increment_enc_before_move[i] = inc_enc[i];
    }
}





